﻿// 验后定权：方差分量估计

using System;
using System.Collections.Generic;
using System.Text;
using MathNet.Numerics.LinearAlgebra;

namespace AdjPlane
{
    //验后估计定权
    public partial class Plane
    {
        //===方差分量估计=====
        protected int iter_VCE;//方差分量估计次数
        protected List<VCE_t> cov_res = new List<VCE_t>();//方差分量估计结果
        
        //赫尔默特方差分量估计
        // ref [4]
        protected void Helmert()  
        {
            int hh = 0;
            var Ninv = N.Inverse();
            var s1 = Ninv * N1;
            var s2 = Ninv * N2;
            double s11 = Convert.ToDouble(nl) - 2 * s1.Trace() + (s1 * s1).Trace();
            double s12 = (s1 * s2).Trace();
            double s21 = s12;
            double s22 = Convert.ToDouble(ns) - 2 * s2.Trace() + (s2 * s2).Trace();
            double[,] So = { { s11, s12 }, { s21, s22 } };//二维数组double[,],里面有逗号！
            var S = Matrix<double>.Build.DenseOfArray(So);
            while (true)
            {
                double[,] Wo = { { PVV1 }, { PVV2 } };
                var Wt = Matrix<double>.Build.DenseOfArray(Wo);
                var theta = S.Inverse() * Wt;//方差分量估计公式
                qvl = theta[0, 0];
                qvs = theta[1, 0];
                //***********迭代**************
                P1 = qvl / qvl * P1;  //权阵迭代
                P2 = qvl / qvs * P2;
                PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
                PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                //***********************
                N1 = A1.Transpose() * P1 * A1;
                N2 = A2.Transpose() * P2 * A2;
                N = N1 + N2;
                double cov_eps = Math.Abs(qvl / qvs - 1);
                if (cov_eps < Eps_VCE || hh == IterMax_VCE) //结束方差分量估计迭代
                    break;
                VCE_t cov = new VCE_t
                {
                    count = iter_VCE,
                    ql = qvl,
                    qs = qvs
                };
                iter_VCE++;
                cov_res.Add(cov);//存储方差分量估计的定权结果
                hh++;
            }
        }
        //舍弃求迹运算,PVV/n
        void SimplfyHe() 
        {
            int hh = 0;
            double s11 = Convert.ToDouble(nl);
            double s12 = 0; double s21 = 0;
            double s22 = Convert.ToDouble(ns);
            double[,] So = { { s11, s12 }, { s21, s22 } };//二维数组double[,],里面有逗号！
            var S = Matrix<double>.Build.DenseOfArray(So);
            while (true)
            {
                double[,] Wo = { { PVV1 }, { PVV2 } };
                var Wt = Matrix<double>.Build.DenseOfArray(Wo);
                var theta = S.Inverse() * Wt;//方差分量估计公式
                qvl = theta[0, 0];
                qvs = theta[1, 0];
                P1 = qvl / qvl * P1;  //权阵迭代
                P2 = qvl / qvs * P2;
                PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
                PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                double cov_eps = Math.Abs(qvl / qvs - 1);
                if (cov_eps < Eps_VCE || hh == IterMax_VCE) //结束方差分量估计迭代
                    break;
                VCE_t cov = new VCE_t
                {
                    count = iter_VCE,
                    ql = qvl,
                    qs = qvs
                };
                iter_VCE++;
                cov_res.Add(cov);//存储方差分量估计的定权结果
                hh++;
            }
        }
        //PVV/r
        protected void SimplfyHe2() 
        {
            int hh = 0;
            var Ninv = N.Inverse();
            var s1 = Ninv * N1; var s2 = Ninv * N2;
            double s11 = nl - s1.Trace();
            double s12 = 0; double s21 = 0;
            double s22 = ns - s2.Trace();
            double[,] So = { { s11, s12 }, { s21, s22 } };//二维数组double[,],里面有逗号！
            var S = Matrix<double>.Build.DenseOfArray(So);
            while (true)
            {
                double[,] Wo = { { PVV1 }, { PVV2 } };
                var Wt = Matrix<double>.Build.DenseOfArray(Wo);
                var theta = S.Inverse() * Wt;//方差分量估计公式
                qvl = theta[0, 0];
                qvs = theta[1, 0];
                P1 = qvl / qvl * P1;  //权阵迭代
                P2 = qvl / qvs * P2;
                PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
                PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                double cov_eps = Math.Abs(qvl / qvs - 1);
                if (cov_eps < Eps_VCE || hh == IterMax_VCE) //结束方差分量估计迭代
                    break;
                VCE_t cov = new VCE_t
                {
                    count = iter_VCE,
                    ql = qvl,
                    qs = qvs
                };
                iter_VCE++;
                cov_res.Add(cov);//存储方差分量估计的定权结果
                hh++;
            }
        }
        //令N为对角阵
        void SimplfyHe3() 
        {
            int hh = 0;
            var N1 = Matrix<double>.Build.SparseOfDiagonalVector((A1.Transpose() * P1 * A1).Diagonal());
            var N2 = Matrix<double>.Build.SparseOfDiagonalVector((A2.Transpose() * P2 * A2).Diagonal());
            var N = N1 + N2;//法方程系数
            double s1 = 0, s2 = 0, s3 = 0, s4 = 0, s5 = 0;
            for (int i = 0; i < N.RowCount; i++)
            {
                s1 += N1[i, i] / N[i, i];
                s2 += N2[i, i] / N[i, i];
                s3 += N1[i, i] * N1[i, i] / (N[i, i] * N[i, i]);
                s4 += N2[i, i] * N2[i, i] / (N[i, i] * N[i, i]);
                s5 += N1[i, i] * N2[i, i] / (N[i, i] * N[i, i]);
            }
            double s11 = Convert.ToDouble(nl) - 2 * s1 + s3;
            double s12 = s5;
            double s21 = s5;
            double s22 = Convert.ToDouble(ns) - 2 * s2 + s4;
            double[,] So = { { s11, s12 }, { s21, s22 } };//二维数组double[,],里面有逗号！
            var S = Matrix<double>.Build.DenseOfArray(So);
            while (true)
            {
                double[,] Wo = { { PVV1 }, { PVV2 } };
                var Wt = Matrix<double>.Build.DenseOfArray(Wo);
                var theta = S.Inverse() * Wt;//方差分量估计公式
                qvl = theta[0, 0];
                qvs = theta[1, 0];
                //***********迭代**************
                P1 = qvl / qvl * P1;  //权阵迭代
                P2 = qvl / qvs * P2;
                PVV2 = (V2.Transpose() * P2 * V2)[0, 0];
                PVV1 = (V1.Transpose() * P1 * V1)[0, 0];
                //***********************
                double cov_eps = Math.Abs(qvl / qvs - 1);
                if (cov_eps < Eps_VCE || hh == IterMax_VCE) //结束方差分量估计迭代
                    break;
                VCE_t cov = new VCE_t
                {
                    count = iter_VCE,
                    ql = qvl,
                    qs = qvs
                };
                iter_VCE++;
                cov_res.Add(cov);//存储方差分量估计的定权结果
                hh++;
            }
        }
        //最小范数二次无偏估计
        void MINQUE() 
        {
            int hh = 0;
            A = A1.Stack(A2);
            L = L1.Stack(L2);
            V = A * X - L;
            Q1 = P1.Inverse().DiagonalStack(Matrix<double>.Build.Dense(ns, ns, 0));
            Q2 = Matrix<double>.Build.Dense(nl, nl, 0).DiagonalStack(P2.Inverse());
            P = P1.DiagonalStack(P2);
            var ANinvAT = A * N.Inverse() * A.Transpose();
            var I = Matrix<double>.Build.DenseIdentity(n);
            var C = P * (I - ANinvAT * P);
            double s11 = (C * Q1 * C * Q1).Trace();
            double s12 = (C * Q1 * C * Q2).Trace();
            double s21 = (C * Q2 * C * Q1).Trace();
            double s22 = (C * Q2 * C * Q2).Trace();
            double[,] So = { { s11, s12 }, { s21, s22 } };
            var S = Matrix<double>.Build.DenseOfArray(So);
            while (true)
            {
                var CV = C * V;
                double w1 = (CV.Transpose() * Q1 * CV)[0, 0];
                double w2 = (CV.Transpose() * Q2 * CV)[0, 0];
                double[,] Wo = { { w1 }, { w2 } };
                var Wt = Matrix<double>.Build.DenseOfArray(Wo);
                var theta = S.Inverse() * Wt;//方差分量估计公式
                qvl = theta[0, 0];
                qvs = theta[1, 0];
                P1 = qvl / qvl * P1;  //权阵迭代
                P2 = qvl / qvs * P2;
                Q1 = P1.Inverse().DiagonalStack(Matrix<double>.Build.Dense(ns, ns, 0));
                Q2 = Matrix<double>.Build.Dense(nl, nl, 0).DiagonalStack(P2.Inverse());
                P = P1.DiagonalStack(P2);
                C = P * (I - ANinvAT * P);
                double cov_eps = Math.Abs(qvl / qvs - 1);
                if (cov_eps < Eps_VCE || hh == IterMax_VCE) //结束方差分量估计迭代
                    break;
                VCE_t cov = new VCE_t
                {
                    count = iter_VCE,
                    ql = qvl,
                    qs = qvs
                };
                iter_VCE++;
                cov_res.Add(cov);//存储方差分量估计的定权结果
                hh++;
            }
        }
        //最小二乘方差分量估计
        // ref [3]
        void LSVCE()  
        {
            int hh = 0;
            A = A1.Stack(A2);
            L = L1.Stack(L2);
            Q1 = P1.Inverse().DiagonalStack(Matrix<double>.Build.Dense(ns, ns, 0));
            Q2 = Matrix<double>.Build.Dense(nl, nl, 0).DiagonalStack(P2.Inverse());
            P = P1.DiagonalStack(P2);
            var ANinvAT = A * N.Inverse() * A.Transpose();
            //A = -1 * A; L = -1 * L;
            var I = Matrix<double>.Build.SparseIdentity(n);
            var R = I - ANinvAT * P;
            var PR = P * R;
            double s11 = (Q1 * PR * Q1 * PR).Trace();
            double s12 = (Q1 * PR * Q2 * PR).Trace();
            double s21 = (Q2 * PR * Q1 * PR).Trace();
            double s22 = (Q2 * PR * Q2 * PR).Trace();
            double[,] So = { { s11, s12 }, { s21, s22 } };
            var S = Matrix<double>.Build.DenseOfArray(So);
            while (true)
            {
                var PRL = P * R * L;
                double w1 = (PRL.Transpose() * Q1 * PRL)[0, 0];
                double w2 = (PRL.Transpose() * Q2 * PRL)[0, 0];
                double[,] Wo = { { w1 }, { w2 } };
                var Wt = Matrix<double>.Build.DenseOfArray(Wo);
                var theta = S.Inverse() * Wt;//方差分量估计公式
                qvl = theta[0, 0];
                qvs = theta[1, 0];
                P1 = qvl / qvl * P1;  //权阵迭代
                P2 = qvl / qvs * P2;
                Q1 = P1.Inverse().DiagonalStack(Matrix<double>.Build.Dense(ns, ns, 0));
                Q2 = Matrix<double>.Build.Dense(nl, nl, 0).DiagonalStack(P2.Inverse());
                P = P1.DiagonalStack(P2);
                R = I - ANinvAT * P;
                double cov_eps = Math.Abs(qvl / qvs - 1);
                if (cov_eps < Eps_VCE || hh == IterMax_VCE) //结束方差分量估计迭代
                    break;
                VCE_t cov = new VCE_t
                {
                    count = iter_VCE,
                    ql = qvl,
                    qs = qvs
                };
                iter_VCE++;
                cov_res.Add(cov);//存储方差分量估计的定权结果
                hh++;
            }
        }
        //极大似然方差分量估计
        void MLE() 
        {
            int hh = 0;
            A = A1.Stack(A2);
            L = L1.Stack(L2);
            P = P1.DiagonalStack(P2);
            Q1 = P1.Inverse().DiagonalStack(Matrix<double>.Build.Dense(ns, ns, 0));
            Q2 = Matrix<double>.Build.Dense(nl, nl, 0).DiagonalStack(P2.Inverse());
            var Ninv = N.Inverse();
            var ANinvATP = A * Ninv * A.Transpose() * P;
            var Q1P = Q1 * P;
            var Q2P = Q2 * P;
            double s1 = (Q1P).Trace() - (Q1P * ANinvATP).Trace();
            double s2 = (Q2P).Trace() - (Q2P * ANinvATP).Trace();
            double[] So = { s1, s2 };
            var S = Matrix<double>.Build.DenseOfDiagonalArray(So);
            while (true)
            {
                double w1 = (V1.Transpose() * P1 * V1)[0, 0];
                double w2 = (V2.Transpose() * P2 * V2)[0, 0];
                double[,] Wo = { { w1 }, { w2 } };
                var Wt = Matrix<double>.Build.DenseOfArray(Wo);
                var theta = S.Inverse() * Wt;
                qvl = theta[0, 0];
                qvs = theta[1, 0];
                P1 = qvl / qvl * P1;  //权阵迭代
                P2 = qvl / qvs * P2;
                double cov_eps = Math.Abs(qvl / qvs - 1);
                if (cov_eps < Eps_VCE || hh == IterMax_VCE) //结束方差分量估计迭代
                    break;
                VCE_t cov = new VCE_t
                {
                    count = iter_VCE,
                    ql = qvl,
                    qs = qvs
                };
                iter_VCE++;
                cov_res.Add(cov);//存储方差分量估计的定权结果
                hh++;
            }

        }
    }
}
